#include "motorwidget.h"
#include "ui_motorwidget.h"

motorWidget::motorWidget(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::motorWidget)
{
    ui->setupUi(this);

    ui->wavewidget->setOpenGl(true);
    ui->wavewidget->legend->setVisible(true);
    ui->wavewidget->axisRect()->insetLayout()->setInsetAlignment(0, Qt::AlignTop | Qt::AlignRight);
    ui->wavewidget->legend->setIconSize(20,10);
    ui->wavewidget->setInteractions(QCP::iSelectPlottables | QCP::iSelectLegend | QCP::iRangeDrag | QCP::iRangeZoom);
    ui->wavewidget->axisRect()->setRangeZoomFactor(1.2,1);
    ui->wavewidget->yAxis->setRange(-1,1);


    xaxis_index = 0;
    xaxis_range = -1000;
    //校准左电机
    connect(ui->pushButton,&QPushButton::clicked,this,[=](){
        SendCMD(0x2A,0x01,0xff,0,0,0);
    });
    //校准右电机
    connect(ui->pushButton_2,&QPushButton::clicked,this,[=](){
         SendCMD(0x2A,0x02,0xff,0,0,0);
    });
    //校准左右电机
    connect(ui->pushButton_3,&QPushButton::clicked,this,[=](){
         SendCMD(0x2A,0x03,0xff,0,0,0);
    });

    connect(ui->pushButton_4,&QPushButton::clicked,this,[=](){
        uint8_t func2,func3;
        if(QString::compare(ui->comboBox->currentText(),"暂停左右电机") == 0)
            func2 = 0x21;
        else if(QString::compare(ui->comboBox->currentText(),"开环") == 0)
            func2 = 0x11;
        else if(QString::compare(ui->comboBox->currentText(),"电流控制") == 0)
            func2 = 0x12;
        else if(QString::compare(ui->comboBox->currentText(),"速度控制") == 0)
            func2 = 0x16;
        else if(QString::compare(ui->comboBox->currentText(),"位置控制") == 0)
            func2 = 0x18;

        if(QString::compare(ui->comboBox_2->currentText(),"左电机") == 0)
            func3 = 0x01;
        else if(QString::compare(ui->comboBox_2->currentText(),"右电机") == 0)
            func3 = 0x02;
        else if(QString::compare(ui->comboBox_2->currentText(),"左右电机") == 0)
            func3 = 0x03;
        SendCMD(0x2A,func2,func3,0,0,0);


    });

    //校准设置Uq Ud
    connect(ui->pushButton_5,&QPushButton::clicked,this,[=](){
        float Uq = ui->doubleSpinBox->value();
        float Ud = ui->doubleSpinBox_2->value();
        uint8_t func3;
        if(QString::compare(ui->comboBox_5->currentText(),"左电机") == 0)
            func3 = 0x01;
        else
            func3 = 0x02;

        SendCMD(0x2A,0x31,func3,Uq,Ud,0);
    });
    //校准波形max min
    connect(ui->pushButton_6,&QPushButton::clicked,this,[=](){

        char offset;
        uint8_t func3;
        float max = ui->doubleSpinBox_3->value();
        float min = ui->doubleSpinBox_4->value();

        if(QString::compare(ui->comboBox_4->currentText(),"左电机") == 0)
            offset = 0;
        else
            offset = 0x04;

        if(QString::compare(ui->comboBox_3->currentText(),"TaTbTc") == 0)
            func3 = 0x01 + offset;
        else if(QString::compare(ui->comboBox_3->currentText(),"IaIbIc") == 0)
            func3 = 0x02 + offset;
        else if(QString::compare(ui->comboBox_3->currentText(),"IqId") == 0)
            func3 = 0x03 + offset;
        else if(QString::compare(ui->comboBox_3->currentText(),"qt") == 0){
            ui->wavewidget->yAxis->setRange(min, max);
            ui->wavewidget->replot();

            return ;
        }
        else
            func3 = 0x04 + offset;



        SendCMD(0x2A,0x41,func3,max,min,0);
    });


    connect(ui->pushButton_7,&QPushButton::clicked,this,&motorWidget::Send_I_Iq);//设置目标电流Iq
    connect(ui->pushButton_9,&QPushButton::clicked,this,&motorWidget::Send_Iq_PID);//设置电流环q的PI参数
    connect(ui->pushButton_11,&QPushButton::clicked,this,&motorWidget::Send_Id_PID);//设置电流环d的PI参数


    connect(ui->pushButton_8,&QPushButton::clicked,this,&motorWidget::Send_Aim_Speed);//设置目标速度
    connect(ui->pushButton_10,&QPushButton::clicked,this,&motorWidget::Send_Speed_PID);//设置速度环的PI参数

    connect(ui->pushButton_12,&QPushButton::clicked,this,&motorWidget::Send_Aim_Position);//设置目标位置·
    connect(ui->pushButton_13,&QPushButton::clicked,this,&motorWidget::Send_Position_PID);//设置位置环的PI参数

    connect(ui->comboBox_7,&QComboBox::currentTextChanged,this,[=](const QString &name){
        if(QString::compare(name,"开环") == 0)
            ui->stackedWidget->setCurrentIndex(0);
        else if(QString::compare(name,"电流环") == 0)
            ui->stackedWidget->setCurrentIndex(1);
        else if(QString::compare(name,"速度环") == 0)
            ui->stackedWidget->setCurrentIndex(2);
        else if(QString::compare(name,"位置环") == 0)
            ui->stackedWidget->setCurrentIndex(3);
    });
}

motorWidget::~motorWidget()
{
    delete ui;
}

//设置目标电流Iq
void motorWidget::Send_I_Iq(){
    uint8_t func3;
    if(QString::compare(ui->comboBox_6->currentText(),"左电机") == 0)
        func3 = 0x01;
    else
        func3 = 0x04;

    float Iq = ui->doubleSpinBox_5->value();

    SendCMD(0x2A,0x13,func3,Iq,0,0);

}

//设置电流环q的PI参数
void motorWidget::Send_Iq_PID(){
    char offset;
    uint8_t func3;
    if(QString::compare(ui->comboBox_6->currentText(),"左电机") == 0)
        offset = 0;
    else
        offset = 0x03;

    func3 = 0x02 + offset;


    float Pq = ui->doubleSpinBox_6->value();
    float Pi = ui->doubleSpinBox_7->value();
    SendCMD(0x2A,0x13,func3,0,Pq,Pi);
}

//设置电流环d的PI参数
void motorWidget::Send_Id_PID(){
    char offset;
    uint8_t func3;
    if(QString::compare(ui->comboBox_6->currentText(),"左电机") == 0)
        offset = 0;
    else
        offset = 0x03;

    func3 = 0x03 + offset;
    float Dq = ui->doubleSpinBox_8->value();
    float Di = ui->doubleSpinBox_9->value();
    SendCMD(0x2A,0x13,func3,0,Dq,Di);
}

//发送目标速度
void motorWidget::Send_Aim_Speed(){
    uint8_t func3;
    if(QString::compare(ui->comboBox_8->currentText(),"左电机") == 0)
        func3 = 0x01;
    else
        func3 = 0x03;

    float speed = ui->doubleSpinBox_10->value();
    SendCMD(0x2A,0x17,func3,speed,0,0);
}
//发送速度PI参数
void motorWidget::Send_Speed_PID(){
    char offset;
    uint8_t func3;
    if(QString::compare(ui->comboBox_6->currentText(),"左电机") == 0)
        offset = 0x02;
    else
        offset = 0x04;

    func3 = offset;

    float speed_q = ui->doubleSpinBox_11->value();
    float speed_i = ui->doubleSpinBox_12->value();

    SendCMD(0x2A,0x17,func3,0,speed_q,speed_i);
}

void motorWidget::Send_Aim_Position(){
    uint8_t func3;
    if(QString::compare(ui->comboBox_9->currentText(),"左电机") == 0)
        func3 = 0x01;
    else
        func3 = 0x03;

    float position = ui->doubleSpinBox_13->value();
    SendCMD(0x2A,0x19,func3,position,0,0);
}
void motorWidget::Send_Position_PID(){
    char offset;
    uint8_t func3;
    if(QString::compare(ui->comboBox_6->currentText(),"左电机") == 0)
        offset = 0x02;
    else
        offset = 0x04;

    func3 = offset;

    float position_q = ui->doubleSpinBox_14->value();
    float position_i = ui->doubleSpinBox_15->value();

    SendCMD(0x2A,0x19,func3,0,position_q,position_i);
}

void motorWidget::SendCMD(uint8_t func1,uint8_t func2,uint8_t func3,float data1,float data2,float data3){
    uint8_t cmd[24];
    for(int i = 0 ;i< 24;i++){
        cmd[i] = 0xff;
    }
    cmd[0] = 0xEF;
    cmd[1] = func1;
    cmd[2] = func2;
    cmd[3] = func3;

    char dd[4];
    memcpy(dd,&data1,sizeof (4));
    cmd[4] = dd[0];
    cmd[5] = dd[1];
    cmd[6] = dd[2];
    cmd[7] = dd[3];

    memcpy(dd,&data2,sizeof (4));
    cmd[8] = dd[0];
    cmd[9] = dd[1];
    cmd[10] = dd[2];
    cmd[11] = dd[3];

    memcpy(dd,&data3,sizeof (4));
    cmd[12] = dd[0];
    cmd[13] = dd[1];
    cmd[14] = dd[2];
    cmd[15] = dd[3];

    cmd[23] = 0xFE;
    emit sendData((char *)cmd,sizeof(cmd));   //发送命令
}

int test_i = 0;
void motorWidget::updateWave(QVector<double> data1,QVector<double> data2,QVector<double> data3,double type){
    if( list_key1.size() > 40000){
        list_key1.clear();
        list_data1.clear();
        list_data2.clear();
        list_data3.clear();
        xaxis_index = 0;
        xaxis_range = -1000;
    }

   for(int i=0;i<data1.size();i++){
       list_key1.push_back(xaxis_index+i);
       list_data1.push_back(data1.at(i));
       list_data2.push_back(data2.at(i));
       list_data3.push_back(data3.at(i));
   }
   xaxis_index += data1.size();
   xaxis_range += data1.size();

   test_i += 1;
   if( test_i ==  1)
       test_i = 0;
   else
       return;
   ui->wavewidget->clearGraphs();

   ui->wavewidget->addGraph();
   ui->wavewidget->addGraph();
   ui->wavewidget->addGraph();
   ui->wavewidget->graph(0)->addData(list_key1,list_data1);
   ui->wavewidget->graph(1)->addData(list_key1,list_data2);
   ui->wavewidget->graph(2)->addData(list_key1,list_data3);
   ui->wavewidget->graph(0)->setPen(QColor(Qt::blue));
   ui->wavewidget->graph(1)->setPen(QColor(Qt::red));
   ui->wavewidget->graph(2)->setPen(QColor(Qt::darkCyan));
   ui->wavewidget->graph(0)->setAntialiasedFill(true);
   ui->wavewidget->graph(1)->setAntialiasedFill(true);
   ui->wavewidget->graph(2)->setAntialiasedFill(true);


   ui->wavewidget->xAxis->setRange(xaxis_range, xaxis_range + 1000);
   //ui->wavewidget->yAxis->rescale();
   setWaveTitle(type);
   ui->wavewidget->replot();
}

void motorWidget::setWaveTitle(double type){

    if( type == 1.1f){
        ui->wavewidget->graph(0)->setName("Ta");
        ui->wavewidget->graph(1)->setName("Tb");
        ui->wavewidget->graph(2)->setName("Tc");
    }
    else if( type == 2.2f){
        ui->wavewidget->graph(0)->setName("Ia");
        ui->wavewidget->graph(1)->setName("Ib");
        ui->wavewidget->graph(2)->setName("Ic");
    }
    else if( type == 3.3f){
        ui->wavewidget->graph(0)->setName("Iq");
        ui->wavewidget->graph(1)->setName("NONE");
        ui->wavewidget->graph(2)->setName("Id");
    }
    else if( type == 4.4f){
        ui->wavewidget->graph(0)->setName("Ialpha");
        ui->wavewidget->graph(1)->setName("NONE");
        ui->wavewidget->graph(2)->setName("Ibeta");
    }
    else{
        ui->wavewidget->graph(0)->setName("angle");
        ui->wavewidget->graph(1)->setName("speed");
        ui->wavewidget->graph(2)->setName("position");
    }

    ui->wavewidget->legend->addElement(0,0,ui->wavewidget->legend->item(0));
    ui->wavewidget->legend->addElement(0,1,ui->wavewidget->legend->item(1));
    ui->wavewidget->legend->addElement(0,2,ui->wavewidget->legend->item(2));

}

